/*
 * Matrix4.h
 *
 *  Created on: 21 mars 2011
 *      Author: seeme
 *      Des matrices de 4*4 quelquchoses.
 */

#ifndef MATRIX4_H_
#define MATRIX4_H_

#include <ostream>
#include <iostream>
#include "Vect4.h"
#include "../../LogManager/Log.hpp"

template <typename U>
class Matrix4{

	public:
		Matrix4<U>(Vect4<U> a, Vect4<U> b, Vect4<U> c, Vect4<U> d){
			setColumn(a, 0);
			setColumn(b, 1);
			setColumn(c, 2);
			setColumn(d, 3);
		}

		Matrix4<U>(){
			setIdentity();
		}

		friend std::ostream& operator<<(std::ostream& out, const Matrix4<U>& v){
			v.print(out);
			return out;
		}

		virtual void print(std::ostream& o) const {
			o << "[";
			for(int i = 0; i < 16; ++i){
				o << values[i] << "   ";
			}
			o << "]\n";
		}

		Matrix4<U>& operator=(U a){
			for(int i = 0; i < 16; ++i)
				values[i] = a;
			return *this;
		}

		Matrix4<U>& operator=(const Matrix4<U>& m){
			if(this != &m){
				for(int i = 0; i < 16; ++i)
					values[i] = m.values[i];
			}
			return *this;
		}

		void setColumn(Vect4<U> vect, unsigned int col){
			if(col > 3)
				Log::e("Matrix4") << "Cannot set column " << col << ": out of bounds";
			values[col] = vect.x;
			values[col+4] = vect.y;
			values[col+8] = vect.z;
			values[col+12] = vect.w;
		}

		void setLine(Vect4<U> vect, unsigned int line){
			if(line > 3)
				Log::e("Matrix4") << "Cannot set line " << line << ": out of bounds";
			values[line*4] = vect.x;
			values[line*4+1] = vect.y;
			values[line*4+2] = vect.z;
			values[line*4+3] = vect.w;
		}

		inline Vect4<U> getColumn(unsigned int col){
			if(col > 3)
				Log::e("Matrix4") << "Cannot getColumn " << col << ": out of bounds";
			return Vect4<U>(values[col], values[col+4], values[col+8], values[col+12]);
		}

		void setIdentity(){
			setColumn(Vect4<U>(1, 0, 0, 0), 0);
			setColumn(Vect4<U>(0, 1, 0, 0), 1);
			setColumn(Vect4<U>(0, 0, 1, 0), 2);
			setColumn(Vect4<U>(0, 0, 0, 1), 3);
		}

		inline U& operator() (unsigned row, unsigned col){
		    if (row >= 4 || col >= 4)
		    	Log::e("Matrix4") << "Cannot get data (" << row << ", " << col << ") : out of bounds";
		    return values[4*row + col];
		}

		inline U operator() (unsigned row, unsigned col) const{
		    if (row >= 4 || col >= 4)
		    	Log::e("Matrix4") << "Cannot get data (" << row << ", " << col << ") : out of bounds";
		    return values[4*row + col];
		}

		inline U& operator() (unsigned id){
			if (id >= 16)
				Log::e("Matrix4") << "Cannot get data (" << id << ") : out of bounds";
			return values[id];
		}

		inline U operator() (unsigned id) const{
			if (id >= 16)
				Log::e("Matrix4") << "Cannot get data (" << id << ") : out of bounds";
			return values[id];
		}

		float* getData(){
			float *k = new float[16];
			for(int i = 0; i < 16; ++i)
				k[i] = values[i];
			return k;
		}

		U values[16];

};

template <typename U>
Matrix4<U> operator*(U k, const Matrix4<U>& m){
	Matrix4<U> ret(m);
	for(int i = 0; i < 16; ++i)
		ret.values[i] *= k;

	return ret;
}

template <typename U>
Matrix4<U> operator*(const Matrix4<U>& m, U k){
	return k*m;
}

template <typename U>
bool operator==(const Matrix4<U>&m1, const Matrix4<U>& m2){
	for(int i = 0; i < 16; ++i){
		if(m1.values[i] != m2.values[i]) return false;
	}

	return true;
}

template <typename U>
bool operator!=(const Matrix4<U>& m1, const Matrix4<U>& m2){
	return !(m1 == m2);
}

template <typename U>
inline const Matrix4<U> operator*(const Matrix4<U>& m1, const Matrix4<U>& m2){
	Matrix4<U> result;

	for(int idx = 0; idx < 4; idx++){
		for(int idy = 0; idy < 4; idy++){
			result(idy, idx) = 0.0f;
			for(int idz = 0; idz < 4; idz++){
				result(idy,idx) += m1(idy,idz) * m2(idz,idx);
			}
		}
	}

	return result;
}

template <typename U>
inline const Vect4<U> operator* (const Matrix4<U>& m1, const Vect4<U>& v){
	Vect4<U> ret;
	ret.x = m1(0, 0)*v.x + m1(0, 1)*v.y + m1(0, 2)*v.z;
	ret.y = m1(1, 0)*v.x + m1(1, 1)*v.y + m1(1, 2)*v.z;
	ret.z = m1(2, 0)*v.x + m1(2, 1)*v.y + m1(2, 2)*v.z;

	return ret;
}


#endif /* MATRIX3_H_ */
